package com.bgyrobot.coordinate.util;

import org.apache.commons.math3.linear.Array2DRowRealMatrix;
import org.apache.commons.math3.linear.RealMatrix;
import org.locationtech.proj4j.ProjCoordinate;

/**
 * 格式转换工具类，用于 double数组，Math3矩阵，Proj4j坐标点 之间的格式
 * @author hesu
 */
public class ConvertUtil {

    /**
     * 坐标列表 => 坐标矩阵，加一列 1.0，维度自适应
     * 
     * 输入二维坐标格式：
     * [[coord1X, coord1Y], [coord2X, coord2Y], [coord3X, coord3Y], ...]
     * 
     * 输入三维坐标格式：
     * [[coord1X, coord1Y, coord1Z], [coord2X, coord2Y, coord2Z], [coord3X, coord3Y, coord3Z], ...]
     */
    public static RealMatrix coordArrayToMatrix(double[][] coords) {
        double[][] nums = new double[coords.length][];
        for (int row = 0; row < nums.length; row++) {
            
            int minlen = coords[row].length;
            nums[row] = new double[minlen + 1];
            
            for (int col = 0; col < minlen; col++) {
                nums[row][col] = coords[row][col];
            }
            nums[row][minlen] = 1.0;
        }
        
        return new Array2DRowRealMatrix(nums);
    }
    
    /**
     * 坐标矩阵 => 坐标列表，去掉最后一列，维度自适应
     * 
     * 返回二维坐标格式：
     * [[coord1X, coord1Y], [coord2X, coord2Y], [coord3X, coord3Y], ...]
     * 
     * 返回三维坐标格式：
     * [[coord1X, coord1Y, coord1Z], [coord2X, coord2Y, coord2Z], [coord3X, coord3Y, coord3Z], ...]
     */
    public static double[][] matrixToCoordArray(RealMatrix matrix) {
        double[][] data = matrix.getData();
        double[][] coords = new double[data.length][];
        
        for (int row = 0; row < coords.length; row++) {
            
            int minlen = data[row].length - 1;
            coords[row] = new double[minlen];
            
            for (int col = 0; col < minlen; col++) {
                coords[row][col] = data[row][col];
            }
        }
        
        return coords;
    }
    
    /**
     * 坐标序列 转 坐标数组，指定来源多少维，自适应
     */
    public static double[][] coordsToCoordArray(ProjCoordinate[] coords, boolean is2d){
        double[][] arr = new double[coords.length][];
        for (int i = 0; i < coords.length; i++) {
            arr[i] = new double[is2d ? 2 : 3];
            arr[i][0] = coords[i].x;
            arr[i][1] = coords[i].y;
            if (! is2d) {
                double z = coords[i].z;
                arr[i][2] = Double.isNaN(z) ? 0 : z;
            }
        }
        return arr;
    }
    
    /**
     * 坐标数组 转 坐标序列，指定来源多少维，自适应
     */
    public static ProjCoordinate[] coordArrayToCoords(double[][] arr, boolean is2d){
        ProjCoordinate[] coords = new ProjCoordinate[arr.length];
        for (int i = 0; i < arr.length; i++) {
            coords[i] = is2d ? new ProjCoordinate(arr[i][0], arr[i][1]) : new ProjCoordinate(arr[i][0], arr[i][1], arr[i][2]);
        }
        return coords;
    }
    
    /**
     * 坐标列表矩阵 => 坐标列表，自动三维
     */
    public static ProjCoordinate[] matrixToCoords(RealMatrix matrix) {
        double[][] nums = matrix.getData();
        ProjCoordinate[] coords = new ProjCoordinate[nums.length];
        for (int row = 0; row < coords.length; row++) {
            coords[row] = new ProjCoordinate();
            for (int col = 0; col < 4; col++) {
                coords[row].x = nums[row][0];
                coords[row].y = nums[row][1];
                coords[row].z = nums[row][2];
            }
        }
        return coords;
    }
    
    /**
     * 坐标列表 => 坐标列表矩阵
     */
    public static RealMatrix coordsToMatrix(ProjCoordinate[] coords) {
        double[][] nums = new double[coords.length][4];
        for (int row = 0; row < coords.length; row++) {
            for (int col = 0; col < 4; col++) {
                nums[row][0] = coords[row].x;
                nums[row][1] = coords[row].y;
                nums[row][2] = Double.isNaN(coords[row].z) ? 0 : coords[row].z;
                nums[row][3] = 1;
            }
        }
        return new Array2DRowRealMatrix(nums);
    }
    
    /**
     * 二维转换矩阵变为三维转换矩阵
     */
    public static RealMatrix transformMatrix2dTo3d(RealMatrix matrix) {
        double[][] data = matrix.getData();
        double[][] nums = new double[4][4];
        
        for (int i = 0; i < 3; i++) {
            int row = i < 2 ? i : 3;
            for (int j = 0; j < 3; j++) {
                int col = j < 2 ? j : 3;
                nums[row][col] = data[i][j];
            }
        }
        for (int i = 0; i < 4; i++) {
            nums[i][2] = 0;
            nums[2][i] = 0;
        }
        nums[2][2] = 1.0;
        
        return new Array2DRowRealMatrix(nums);
    }
    
    /**
     * 二维转换矩阵变为三维转换矩阵
     */
    public static RealMatrix transformMatrix3dTo2d(RealMatrix matrix) {
        double[][] data = matrix.getData();
        double[][] nums = new double[3][3];
        
        for (int i = 0; i < 3; i++) {
            int row = i < 2 ? i : 3;
            for (int j = 0; j < 3; j++) {
                int col = j < 2 ? j : 3;
                nums[i][j] = data[row][col];
            }
        }
        
        return new Array2DRowRealMatrix(nums);
    }
}
